# The MIT License (MIT)
#
# Copyright (c) 2021 , 01Studio
from machine import Pin,PWM

import time

def clamp_number(num,a,b):
    return max(min(num, max(a, b)), min(a, b))

class CAR():
    
    #初始化
    def __init__(self):
        
        #电机1
        self.M1_P = PWM(Pin(14), freq=1000, duty=0) 
        self.M1_N = PWM(Pin(27), freq=1000, duty=0)
        
        #电机2
        self.M2_P = PWM(Pin(25), freq=1000, duty=0) 
        self.M2_N = PWM(Pin(26), freq=1000, duty=0)
        
        #电机3
        self.M3_P = PWM(Pin(32), freq=1000, duty=0) 
        self.M3_N = PWM(Pin(33), freq=1000, duty=0)

        #电机4
        self.M4_P = PWM(Pin(13), freq=1000, duty=0) 
        self.M4_N = PWM(Pin(12), freq=1000, duty=0)

        #电机速度，默认最大
        self.duty=1023
        self.angle = 0
     
    #调速
    def setspeed(self,speed):
        self.duty = speed
    #前进    
    def forward(self):
        speed=self.duty

        self.M1_P.duty(speed)
        self.M1_N.duty(0)    
        self.M2_P.duty(speed)
        self.M2_N.duty(0)
        self.M3_P.duty(speed)
        self.M3_N.duty(0)
        self.M4_P.duty(speed)
        self.M4_N.duty(0)
    
    #后退
    def backward(self):
        speed=self.duty

        self.M1_P.duty(0)
        self.M1_N.duty(speed)    
        self.M2_P.duty(0)
        self.M2_N.duty(speed)
        self.M3_P.duty(0)
        self.M3_N.duty(speed)
        self.M4_P.duty(0)
        self.M4_N.duty(speed)

    #左转
    def turn_left(self, mode=0):
        speed=self.duty

        #普通转向
        if mode == 0: 
            self.M1_P.duty(0)
            self.M1_N.duty(0)    
            self.M2_P.duty(speed)
            self.M2_N.duty(0)
            self.M3_P.duty(speed)
            self.M3_N.duty(0)
            self.M4_P.duty(0)
            self.M4_N.duty(0)
        
        #大幅度转向
        elif mode ==1:
            self.M1_P.duty(0)
            self.M1_N.duty(speed)    
            self.M2_P.duty(speed)
            self.M2_N.duty(0)
            self.M3_P.duty(speed)
            self.M3_N.duty(0)
            self.M4_P.duty(0)
            self.M4_N.duty(speed)

    #右转
    def turn_right(self, mode=0):
        speed=self.duty
        self.s_forward=0
        self.s_backward=0
        self.s_left=0
        self.s_right=1
        #普通转向
        if mode == 0:
            self.M1_P.duty(speed)
            self.M1_N.duty(0)    
            self.M2_P.duty(0)
            self.M2_N.duty(0)
            self.M3_P.duty(0)
            self.M3_N.duty(0)
            self.M4_P.duty(speed)
            self.M4_N.duty(0)
        #大幅度转向
        elif mode == 1: 
            self.M1_P.duty(speed)
            self.M1_N.duty(0)    
            self.M2_P.duty(0)
            self.M2_N.duty(speed)
            self.M3_P.duty(0)
            self.M3_N.duty(speed)
            self.M4_P.duty(speed)
            self.M4_N.duty(0)

    #停止
    def stop(self):
        self.M1_P.duty(0)
        self.M1_N.duty(0)    
        self.M2_P.duty(0)
        self.M2_N.duty(0)
        self.M3_P.duty(0)
        self.M3_N.duty(0)
        self.M4_P.duty(0)
        self.M4_N.duty(0)

    #####################
    # Mecanum麦克纳母轮
    #####################   
    def up(self): #上移
        
        speed=self.duty        
        self.M1_P.duty(speed)
        self.M1_N.duty(0)    
        self.M2_P.duty(speed)
        self.M2_N.duty(0)
        self.M3_P.duty(speed)
        self.M3_N.duty(0)
        self.M4_P.duty(speed)
        self.M4_N.duty(0)
        
    def down(self): #下移
        
        speed=self.duty 
        self.M1_P.duty(0)
        self.M1_N.duty(speed)    
        self.M2_P.duty(0)
        self.M2_N.duty(speed)
        self.M3_P.duty(0)
        self.M3_N.duty(speed)
        self.M4_P.duty(0)
        self.M4_N.duty(speed)

    def left(self): #左移
        
        speed=self.duty 
        self.M1_P.duty(0)
        self.M1_N.duty(int(speed * 0.8))    
        self.M2_P.duty(int(speed * 2))
        self.M2_N.duty(0)
        self.M3_P.duty(speed)
        self.M3_N.duty(0)
        self.M4_P.duty(0)
        self.M4_N.duty(0)

    def right(self): #右移
        
        speed=self.duty 
        self.M1_P.duty(int(speed * 2))
        self.M1_N.duty(0)    
        self.M2_P.duty(0)
        self.M2_N.duty(int(speed * 0.8))
        self.M3_P.duty(0)
        self.M3_N.duty(0)
        self.M4_P.duty(speed)
        self.M4_N.duty(0)

    def up_left(self): #左前移
        
        speed=self.duty 
        self.M1_P.duty(int(speed * 1.4))
        self.M1_N.duty(0)    
        self.M2_P.duty(int(speed * 0.6))
        self.M2_N.duty(0)
        self.M3_P.duty(int(speed * 0.8))
        self.M3_N.duty(0)
        self.M4_P.duty(int(speed * 1.2))
        self.M4_N.duty(0)

    def up_right(self): #右前移
        
        speed=self.duty 
        self.M1_P.duty(int(speed * 0.6))
        self.M1_N.duty(0)    
        self.M2_P.duty(int(speed * 1.4))
        self.M2_N.duty(0)
        self.M3_P.duty(int(speed * 1.2))
        self.M3_N.duty(0)
        self.M4_P.duty(int(speed * 0.8))
        self.M4_N.duty(0)

    def down_left(self): #左后移
        
        speed=self.duty 
        self.M1_P.duty(0)
        self.M1_N.duty(speed)    
        self.M2_P.duty(0)
        self.M2_N.duty(0)
        self.M3_P.duty(0)
        self.M3_N.duty(speed)
        self.M4_P.duty(0)
        self.M4_N.duty(0)
    
    def down_right(self): #右后移
        
        speed=self.duty 
        self.M1_P.duty(0)
        self.M1_N.duty(0)    
        self.M2_P.duty(0)
        self.M2_N.duty(speed)
        self.M3_P.duty(0)
        self.M3_N.duty(0)
        self.M4_P.duty(0)
        self.M4_N.duty(speed)

    def clockwise(self): #顺时针旋转
        
        speed=self.duty
        angle= self.angle      
        self.M1_P.duty(0)
        self.M1_N.duty(speed)    
        self.M2_P.duty(speed)
        self.M2_N.duty(0)
        self.M3_P.duty(speed)
        self.M3_N.duty(0)
        self.M4_P.duty(0)
        self.M4_N.duty(speed)
        time.sleep_ms(angle)
        self.stop()

    def Counterclockwise(self): #逆时针旋转
        
        speed=self.duty
        angle= self.angle
        self.M1_P.duty(speed)
        self.M1_N.duty(0)    
        self.M2_P.duty(0)
        self.M2_N.duty(speed)
        self.M3_P.duty(0)
        self.M3_N.duty(speed)
        self.M4_P.duty(speed)
        self.M4_N.duty(0)
        time.sleep_ms(angle)
        self.stop()

        
    def turn(self,speed):
        if (speed[0] >= 0):
            self.M1_P.duty(speed[0])
            self.M1_N.duty(0)
        else :
            self.M1_P.duty(0)
            self.M1_N.duty(abs(speed[0]))
        if (speed[1] >= 0):
            self.M2_P.duty(speed[1])
            self.M2_N.duty(0)
        else :
            self.M2_P.duty(0)
            self.M2_N.duty(abs[speed[1]])
        if (speed[2] >= 0):
            self.M3_P.duty(speed[2])
            self.M3_N.duty(0)
        else :
            self.M3_P.duty(0)
            self.M3_N.duty(abs(speed[2]))
        if (speed[3] >= 0):
            self.M4_P.duty(speed[3])
            self.M4_N.duty(0)
        else :
            self.M4_P.duty(0)
            self.M4_N.duty(abs(speed[3]))          
#     #五路光电传感器
#     def T1(self):
#         
#         return self.t1.value()
# 
#     def T2(self):
#         
#         return self.t2.value()
#     
#     def T3(self):
#         
#         return self.t3.value()
# 
#     def T4(self):
#         
#         return self.t4.value()
#     
#     def T5(self):
#         
#         return self.t5.value()



